
#include <PID_v1.h>

// 定义传感器引脚
#define LEFT_SENSOR 16
#define CENTER_SENSOR 18
#define RIGHT_SENSOR 17

// 定义电机引脚
#define MOTOR_A1 5
#define MOTOR_A2 6
#define MOTOR_B1 9
#define MOTOR_B2 10

// 设置PID参数
double Setpoint = 0; // 目标值为0，表示车辆应当保持在中心位置
double Kp = 2;       // 比例系数
double Ki = 5;       // 积分系数
double Kd = 1;       // 微分系数

double Input, Output;
PID pid(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
int leftSensorValue, centerSensorValue, rightSensorValue,level=0;
int leftSpeed,rightSpeed;
void setup() {
  Serial.begin(9600);
  // 配置电机引脚为输出模式
  pinMode(MOTOR_A1, OUTPUT);
  pinMode(MOTOR_A2, OUTPUT);
  pinMode(MOTOR_B1, OUTPUT);
  pinMode(MOTOR_B2, OUTPUT);
  // 配置传感器引脚为输入模式
  pinMode(CENTER_SENSOR, INPUT);
  pinMode(LEFT_SENSOR, INPUT);
  pinMode(RIGHT_SENSOR, INPUT);

  // 初始化PID控制器
  pid.SetMode(AUTOMATIC);
  pid.SetOutputLimits(-255, 255);

  // 输出PID参数信息
  Serial.println("PID Tuning:");
  Serial.print("Kp: ");
  Serial.println(Kp);
  Serial.print("Ki: ");
  Serial.println(Ki);
  Serial.print("Kd: ");
  Serial.println(Kd);
}

void loop() {
  //初始化PID
  init_PID();

  // 使用PID控制器计算输出
  pid.Compute();

  //控制转速和方向
  run();

switch (level) {
    case 0: // 关卡0的逻辑
      levelZeroLogic();
      break;
    case 1: // 关卡1的逻辑
      levelOneLogic();
      break;
    case 2: // 关卡2的逻辑
      levelTwoLogic();
      break;
    // 添加更多关卡的逻辑...
    default:
      // 如果level不匹配任何已知关卡，则执行默认逻辑
      defaultLogic();
      break;
  }

  
}

  // 关卡0的逻辑
  void levelZeroLogic() {
  //起点 直行
  car_Direct();
  }
  
  // 关卡1的逻辑
  void levelOneLogic() {
  // 在这里编写关卡1的逻辑
  
  }
  
  // 关卡2的逻辑
  void levelTwoLogic() {
  // 在这里编写关卡2的逻辑
  }
  
  // 默认逻辑
  void defaultLogic() {
  // 在这里编写默认的逻辑，处理未知关卡
  }

  //判断关卡
  void sel_level(){
  if (!(digitalRead(16))&&!(digitalRead(17))&&!(digitalRead(18)))
  {
      level++;
  }
  
  }

  void init_PID(){
    // 读取传感器数据
    leftSensorValue = digitalRead(LEFT_SENSOR);
    centerSensorValue = digitalRead(CENTER_SENSOR);
    rightSensorValue = digitalRead(RIGHT_SENSOR);
    
    // 计算传感器数据的误差
    double error = 0.5 * (double)(rightSensorValue - leftSensorValue) + (double)centerSensorValue - 0.5;
  
    // 将误差归一化到 [-1, 1] 范围内
    Input = error / 1023.0 * 2.0;
  }

  void run(){
    // 根据PID输出来控制电机转向
    int leftSpeed = constrain(150 + Output, 0, 255); // 左轮速度
    int rightSpeed = constrain(150 - Output, 0, 255); // 右轮速度
    
    if(!(digitalRead(16))&&!(digitalRead(17))&&!(digitalRead(18))&&level==0){
        //全黑并且关卡为0 直行
        car_Direct();
    }
    else if(!(digitalRead(16))){
        // 控制右轮转速增加，左转
        car_left();
        }
    else if(!(digitalRead(17))){
        // 控制左轮转速增加，右转
        car_right();
        }
    else if(digitalRead(16)&&digitalRead(17)&&digitalRead(18)){
      //全白 停止
      car_stop();
        }
    else if (!(digitalRead(16))&&!(digitalRead(17))&&!(digitalRead(18))&&level==5){
       //终点 停止
      car_stop();
    }
    else {
      //直行
      car_Direct();  
        }
    // 输出调试信息
    Serial.print("Input: ");
    Serial.print(Input);
    Serial.print(" Output: ");
    Serial.println(Output);
  
    delay(10); // 延迟一段时间
  }
  //停止函数
  void car_stop(){
      analogWrite(MOTOR_A1, 0);
      digitalWrite(MOTOR_A2, LOW);
      analogWrite(MOTOR_B1, 0);
      digitalWrite(MOTOR_B2, LOW);
    }
  //直行函数
  void car_Direct(){
      analogWrite(MOTOR_A1, leftSpeed);
      digitalWrite(MOTOR_A2, LOW);
      analogWrite(MOTOR_B1, rightSpeed);
      digitalWrite(MOTOR_B2, LOW);
    }
  //右转函数
  void car_right(){
      analogWrite(MOTOR_A1, 0);
      digitalWrite(MOTOR_A2, leftSpeed);
      analogWrite(MOTOR_B1, rightSpeed);
      digitalWrite(MOTOR_B2, LOW);
    }
  //左转函数
  void car_left(){
      analogWrite(MOTOR_A1, leftSpeed);
      digitalWrite(MOTOR_A2, LOW);
      analogWrite(MOTOR_B1, 0);
      digitalWrite(MOTOR_B2, rightSpeed);
    }
